#ifndef SPIDER_LEG_KINEMATICS_H
#define SPIDER_LEG_KINEMATICS_H

#include "stm32f1xx_hal.h"
#include <math.h>

/************************** 参考《main.pdf》2.1节参数 **************************/
// 单腿杆长（mm）：对应《main.pdf》L₁（舵机1→2）、L₂（舵机2→3）、L₃（舵机3→脚尖）
#define LEG_L1 28.0f
#define LEG_L2 42.6f
#define LEG_L3 89.07f

// 坐标系定义（《main.pdf》图2-1）：X向前、Y侧方、Z向上（脚尖向下时z为负）
// 舵机角度基准（用户需求）：foot=0°→脚尖向下，对应《main.pdf》θ₃=-90°
// 修正后（正确）：
#define USER_FOOT_TO_DOC_THETA3(foot) (-foot - 90.0f)  // 增加负号反转方向
#define DOC_THETA3_TO_USER_FOOT(theta3) (-(theta3 + 90.0f))  // 保持反向映射一致

// 舵机范围（用户设定：-90°~90°）
#define SERVO_MIN (-90.0f)
#define SERVO_MAX 90.0f

















/************************** 数据结构（参考arm.cpp封装） **************************/
// 解算状态（参考arm.cpp返回逻辑）
typedef enum {
    LEG_OK = 0,          // 解算成功
    LEG_ERR_PARAM = 1,   // 参数空指针
    LEG_ERR_WORKSPACE = 2// 超出《main.pdf》2.1.2节工作空间
} LegStatus;

// 脚尖坐标（《main.pdf》2.1.1节D4坐标）
typedef struct {
    float x;  // 向前为正
    float y;  // 侧方为正
    float z;  // 向上为正（脚尖向下→z负）
} D4Coord;

// 舵机控制角度（用户直接使用）
typedef struct {
    float body;  // 底座关节（对应《main.pdf》θ₁）
    float limb;  // 大臂关节（对应《main.pdf》θ₂）
    float foot;  // 小臂关节（对应《main.pdf》θ₃+90°）
} ServoAngles;

// 步态分组（参考《main.pdf》3.1节二步态）
typedef enum {
    GAIT_GROUP_A = 0,  // 腿1、3、5（支撑相/摆动相）
    GAIT_GROUP_B = 1   // 腿2、4、6（支撑相/摆动相）
} GaitGroup;

/************************** 解算函数（整合参考逻辑） **************************/
// 1. 正解：舵机角度→脚尖坐标（《main.pdf》2.1.1节式2-3）
LegStatus Leg_ForwardKinematics(ServoAngles servo_ang, D4Coord* d4_coord);

// 2. 逆解：脚尖坐标→舵机角度（《main.pdf》2.1.2节几何法）
LegStatus Leg_InverseKinematics(D4Coord target_coord, ServoAngles* servo_ang);

// 3. 步态组解算（参考《main.pdf》3.1节，支持二步态分组验证）
LegStatus Leg_GaitGroupSolve(GaitGroup group, D4Coord target_coords[3], ServoAngles servo_angs[3]);

// 4. 闭环验证（参考arm.cpp Servo_VerifyCloseLoop，确保正逆解一致）
uint8_t Leg_CloseLoopVerify(ServoAngles input_ang, float max_err);







// 腿部安装参数（严格对齐《main.pdf》附录MATLAB代码的Leg.base定义）
// 全局坐标系规则：原点=机器人底盘中心，X正=向前，Y正=向左，Z正=向上
typedef struct {
    float offset_x;   // 底座在全局X轴偏移（正=前方，负=后方，对应《main.pdf》Leg.base的x分量）
    float offset_y;   // 底座在全局Y轴偏移（正=左侧，负=右侧，对应《main.pdf》Leg.base的y分量）
    float offset_z;   // 底座在全局Z轴偏移（通常为0）
    float mount_angle;// 腿部局部X轴与全局X轴的夹角（度，绕Z轴旋转）
} LegMountParam;

// 6条腿的安装参数（示例，需根据实际结构调整）
extern LegMountParam leg_mount_params[6];  // 索引0~5对应腿1~6

// 坐标转换函数声明
void Leg_LocalToGlobal(uint8_t leg_idx, D4Coord local_coord, D4Coord* global_coord);
void Leg_GlobalToLocal(uint8_t leg_idx, D4Coord global_coord, D4Coord* local_coord);

// 全局坐标系解算函数声明
LegStatus Leg_ForwardKinematicsGlobal(uint8_t leg_idx, ServoAngles servo_ang, D4Coord* global_coord);
LegStatus Leg_InverseKinematicsGlobal(uint8_t leg_idx, D4Coord global_coord, ServoAngles* servo_ang);
LegStatus Leg_GaitGroupSolveGlobal(GaitGroup group, D4Coord global_coords[3], ServoAngles servo_angs[3]);
#endif // SPIDER_LEG_KINEMATICS_H